# ugv_chassis_control/chassis_cmd_node.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64, Float64MultiArray
from actuator_msgs.msg import Actuators
from geometry_msgs.msg import Twist
from math import tan
from functools import partial
from rclpy.qos import QoSProfile, QoSReliabilityPolicy

class ChassisCmdNode(Node):
    def __init__(self):
        super().__init__('chassis_cmd_node')
        self.declare_parameter('ugv_ids', ['ugv1'])
        self.ugv_ids = self.get_parameter('ugv_ids').value
        self.cmd_pub = {}
        for ugv_id in self.ugv_ids:
            sub_topic = f'/{ugv_id}/actuators/chassis/in/to_can_bus_cmd'
            pub_topic = f'/model/{ugv_id}/cmd_vel'
            self.sub = self.create_subscription(
                Float64MultiArray,
                sub_topic,
                partial(self.cmd_callback, ugv_id=ugv_id),
                10
            )
        

            self.cmd_pub[ugv_id] = self.create_publisher(
                Twist,
                pub_topic,
                10
            )

        '''
        # 发布转向
        self.servo_pub = self.create_publisher(Float64, "/model/rover_ackermann/steer_angle",10)
        # 发布四轮速度控制（Actuators）
        self.motor_pub = self.create_publisher(Actuators, "/model/rover_ackermann/command/motor_speed", 10)
        '''
        # self.cmd_pub = self.create_publisher(Twist, "cmd", 10)
        self.get_logger().info("✅ chassis_cmd_node started")

    def cmd_callback(self, msg: Float64MultiArray, ugv_id: str):
        if len(msg.data) != 5:
            self.get_logger().warn("⚠️ Expect 5 elements: [mode, shift, steer, speed, brake]")
            return

        mode, shift, steer_angle, speed, brake = msg.data
        
        """
        param speed: speed of vehicle [m/s]
        param angle: steering angle of vehicle [rad]
        param shift: shift of vehicle [1:D, 2:N, 3:R]
        param brake: brake of vehicle [0:off, 1:on]
        enum ShiftLevel : uint8_t { D = 1, N, R };
        enum DriveMode : uint8_t { None = 0, Auto, PAD, Remote };
        enum DriveMode : uint8_t {
            None = 0,   // 无控制
            Auto = 1,   // 自动驾驶
            PAD = 2,    // 平板控制
            Remote = 3  // 遥控
        };
        enum ShiftLevel : uint8_t {
            D = 1,  // 前进挡
            N = 2,  // 空挡
            R = 3   // 倒挡
        };
        """
        need_stop = (brake == 1 or mode == 0 or shift == 2)
        if need_stop:
            speed = 0.0
            velocity = speed
            steer_angle = 0.0
            angular_z = steer_angle
        if brake == 0:
            angular_z = steer_angle
            if shift == 1:
                velocity = speed
            elif shift == 3:
                velocity = -speed
            else:
                velocity = 0.0

        twist_msg = Twist()
        twist_msg.linear.x = velocity 
        twist_msg.angular.z = angular_z
        '''
        self.servo_pub.publish(Float64(data=steer_angle))
        motor_msg = Actuators()
        motor_msg.velocity = [velocity, velocity, velocity, velocity]
        self.motor_pub.publish(motor_msg)
        self.get_logger().info(f"🚗 steer: {steer_angle:.2f} rad | speed: {speed:.2f} m/s")
        '''
        self.cmd_pub[ugv_id].publish(twist_msg)
        # self.get_logger().info(f"🚗 cmd_vel: linear={velocity:.2f} m/s, angular={angular_z:.2f} rad/s")

    

    
    
def main(args=None):
    rclpy.init(args=args)
    node = ChassisCmdNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
